// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#include "ff_utils.h"
#include "main.h"
#include "../utils/util.h"
#include "../utils/button.h"
#include "../utils/LCD_driver.h"
#include "../utils/LCD_functions.h"



void showLinearDist(void)  
{
    unsigned char adcLDist, adcMDist, adcRDist;
	unsigned char LDist, MDist, RDist;
    //int sensorDifference;
   
    disable_JTAG();                  // Disable JTAG port, making pins available for other use.
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))       //do the folling block until enter is pressed
    {
        // get line sensor values
		adcRDist = GetADC(RightDistSensor);
		adcLDist = GetADC(LeftDistSensor);
		adcMDist = GetADC(MiddleDistSensor);
		
		RDist = (2000/adcRDist);
		LDist = (2000/adcLDist);
		MDist = (2000/adcMDist);	
		//RDist = ((7947/(adcRDist-15))-4);
		//LDist = ((7947/(adcLDist-15))-4);	
//		sensorDifference = (LDist - RDist);
//      show2(adcL, sensorDifference);
        show3hex(LDist,MDist,RDist);
        TimerWait(100);
    }
}


/**********************************************************************************************************
*
* 	Forward - Goes forward for a time
*
***********************************************************************************************************/

void Forward(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed);
		SetRightMotorPWM(Speed);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


//********************
//*	GoToHeading()	*
//********************/
int GoToHeading(unsigned char TargetHeading,unsigned char Speed)
{
	unsigned char retries=0, diff=0;
	
	if(!ReadCompass())
	{
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);
		printLCD("bad compass reading");
		TimerWait(10000);
		return(FALSE);
	}
	if (Heading > TargetHeading)
		{
		diff = Heading - TargetHeading;
		}
	else if (Heading == TargetHeading)
		{
		diff = 0;
		}
	else{
		diff = TargetHeading - Heading;
		}
	while (diff >= 4)
	{//Left Turns
		//Left turns that don't cross zero
		if((Heading > TargetHeading) && ((Heading - TargetHeading) <= 128)){	
			while((Heading > TargetHeading) && ((Heading -TargetHeading)>4)&& ((Heading - TargetHeading) <= 128))
			{//Turn at decreasing speed until within 2 of Target
				//printLCD("Left1");
				diff = Heading - TargetHeading;
				show1hex(diff);
				SetLeftMotorPWM(-((diff/(15-Speed))+4));
				SetRightMotorPWM((diff/(15-Speed))+2);
				retries	= 0;
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("L1Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);
			printLCD("L1 Done");
			return(TRUE);//Exit
		}	
		//Left turns that cross zero 
		else if	((Heading < TargetHeading) && ((TargetHeading - Heading) >= 128)){
			while((Heading < TargetHeading) && ((255 - TargetHeading + Heading)>4)&& ((TargetHeading - Heading) >= 128))
			{//Turn at decreasing speed until within 2 of traget
				//printLCD("Left2");	
				diff = (255 - TargetHeading + Heading);
				show1hex(diff);
				SetLeftMotorPWM(-((diff/(15-Speed))+4));
				SetRightMotorPWM((diff/(15-Speed))+2);
				retries	= 0;
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("L2Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			//SetRightMotorPWM(0);
			//SetLeftMotorPWM(0);
			printLCD("L2 Done");		
		}
	//Right Turns
		//Right turn that do not cross zero
		else if((Heading < TargetHeading) && ((TargetHeading - Heading) < 128)){
			while((Heading < TargetHeading) && ((TargetHeading - Heading)>4) && ((TargetHeading - Heading) < 128))
			{//Turn at descreasing speed until within 2 of Target
				//printLCD("Right1");
				diff = (TargetHeading - Heading);
				show1hex(diff);
				SetLeftMotorPWM((diff/(15-Speed))+2);
				SetRightMotorPWM(-((diff/(15-Speed))+4));		
				retries	= 0;
				while(!ReadCompass())
				{
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);					
					if(++retries > 5)
					{
						printLCD("R1Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}		
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);	
			printLCD("R1 Done");
			return(TRUE);//exit
		}	
		//right turn that do cross zero
		else if	((Heading > TargetHeading) && ((Heading - TargetHeading) > 128)){
			while((Heading > TargetHeading) && ((255 - Heading + TargetHeading)>4) && ((Heading - TargetHeading) > 128))
			{//Turn with decreasing speed until within 2 of target
				//printLCD("Right2");
				diff = (255 - Heading + TargetHeading);
				show1hex(diff);
				SetLeftMotorPWM((diff/(15-Speed))+2);
				SetRightMotorPWM(-((diff/(15-Speed))+4));
				retries	= 0;
				while(!ReadCompass())
				{						
					SetRightMotorPWM(0);
					SetLeftMotorPWM(0);
					printLCD("Compass Recheck");
					TimerWait(3000);
					if(++retries > 5)
					{
						printLCD("R2Error");
						TimerWait(3000);					
						return(FALSE);
					}
				}
			}
			//SetRightMotorPWM(0);
			//SetLeftMotorPWM(0);
			printLCD("R2 Done");		
		}
		else{
			//else bad things
			SetRightMotorPWM(0);//to bottom outside while loop
			SetLeftMotorPWM(0);
			printLCD("Should never happen");
			TimerWait(5000);
			show2hex(Heading,TargetHeading);
			TimerWait(5000);
			return(FALSE);
			}
		SetRightMotorPWM(0);//to bottom outside while loop
		SetLeftMotorPWM(0);
		//printLCD("Target");
		//TimerWait(500);
		return(TRUE);
	}
	SetRightMotorPWM(0);//to bottom outside while loop
	SetLeftMotorPWM(0);
	//printLCD("Target");
	//TimerWait(500);
	return(TRUE);
}



void RTurn90(void)
{
	unsigned char	TargetHeading;

	if (ReadCompass())
	{
		TargetHeading = Heading + 28;
	
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}
		if(!GoToHeading(TargetHeading,7))
		{
			printLCD("Error");
		}		
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);	
	}	
}




int ScanRoomForFlame(void)
//this will see if there is flame in the room
//changed to do 540 degrees

{
	unsigned char	EntryHeading,
					ExitHeading,
					HotPixel,
					retries,
					Temperature,
					Target;
//					index;
	int 			NumberOfTurns;
	
	
	// get entry heading
	retries	= 0;
	while(!ReadCompass())
	{
		if(++retries > 5)
			return(FALSE);
	}
	
	NumberOfTurns	= 0;

	EntryHeading	= Heading;
	
	if (EntryHeading >= 128) {
		ExitHeading = EntryHeading - 128;
	}
	else if(EntryHeading < 128) {
		ExitHeading = EntryHeading + 128;
	}
	
	Target	= Heading;
	
	while (!(getkey() == 1))
	{
		// look for hot pixel
		GetHotPixel(&HotPixel,&Temperature);
		if(Temperature > 100) // If there is a flame, returns true
		{
			printLCD("Sees Flame");
			TimerWait(10000);				
			show2hex(HotPixel,Temperature);
		    TimerWait(2000);		
			//sendNumber(HotPixel);
			//sendChar(',');
			//sendNumber(Temperature);
			//sendString("\r\n");
			Target = Target + ((4 - HotPixel) * 3); // try to leave function centered on flame
			GoToHeading(Target,4);
//			GoToFlame();
			return(TRUE);
		}
		else
		{
			Target = Target + 14; // 28 is 40 degrees, range of the sensor	
			GoToHeading(Target,10);
			NumberOfTurns++;
			
			if(NumberOfTurns >= 27) // If a full turn is completed, return to orignal heading
			{	
				show2hex(EntryHeading, ExitHeading);
				TimerWait(5000);
				printLCD("No Flame");
				TimerWait(1000);												
				return(FALSE);
			}
		}
	}
	return(FALSE);
}


/**********************************************************************************************************
*
* 	Goes To Room 1 Trinity
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom1(void)
{
	Forward(9,UNTIL_TIME,TimerMsCur() + 400);	
	
	RWallFollow(0x1E,UNTIL_LINESENSOR_LOWER,0xc8);
 	
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(100);

	printLCD("SQUARE");
	GoToHeading((RoomOneEnterHeading), 5); 

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
 	TimerWait(500);

	printLCD("FWD");
	Forward(8,UNTIL_TIME,TimerMsCur() + 200);	
	TimerWait(500);

	printLCD("SCAN");
	TimerWait(500);	

	if(ScanRoomForFlame())
	{	
		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(10000);
		HomeFrom1();
	}
	else
	{
		printLCD("Back from Check for flame");	
		TimerWait(5000);	
		printLCD("Turn to leave");	
		GoToHeading((RoomOneEnterHeading+128), 2); 
		TimerWait(5000);
		Forward(6,UNTIL_LINESENSOR_LOWER,0xEE);
		TimerWait(20000);
		// goes To Room 2 from room 1 door white line
		//ToRoom2();
	}
	printLCD("Exit");
}
//#########################################################################################################




/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Going To The Rooms (Canada)

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/





/**********************************************************************************************************
*
* 	Goes To Room 2 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom2(void)//go to room 2 from room 1
{
	Forward(8,UNTIL_TIME,TimerMsCur() + 700);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);// follows right wall untill line sensor sees doorway 
	GoToHeading((RoomTwoEnterHeading - 8), 7);
	Forward(8,UNTIL_TIME,TimerMsCur() + 1000);
	printLCD("SQUARE");
	TimerWait(1000);	
	printLCD("SCAN ROOM");
	TimerWait(1000);

	if(ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom2();
/*		unsigned char	FlameHeading	= Heading;

		GoToFlame();
		/// *** works? check or replace with home from 2
		Forward(-8,UNTIL_TIME,TimerMsCur() + 1000);
		TimerWait(2000);
		GoToHeading(FlameHeading + 128, 8);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(2000);
		GoToHeading(OriginalHeading + 32, 8);
		TimerWait(2000);
		Forward(8,UNTIL_LEFTSENSOR_GREATER,0x30);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway
		TimerWait(2000);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		//Go Home From Room 1
		TimerWait(2000);*/
	}
	else
	{
		printLCD("Back from Check for flame");	
		TimerWait(500);
		GoToHeading((RoomTwoEnterHeading+ 132), 7);
		TimerWait(500);		
		Forward(8,UNTIL_LINESENSOR_LOWER,0x40);
		//printLCD("Done");
		//TimerWait(1000);
		ToRoom3();
	}
	printLCD("Exit");
}		
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 3 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom3(void)//go to room 3 from room 2
{
    Forward(8,UNTIL_TIME,TimerMsCur() + 500);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);
	TimerWait(1000);
    Forward(8,UNTIL_TIME,TimerMsCur() + 100);	
	GoToHeading((RoomThreeEnterHeading), 8);
	// *** change to lower angle to center in doorway
	//TimerWait(1000);
	Forward(8, UNTIL_MIDDLESENSOR_GREATER,0x25);
	if (ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom3(); 
	}
	else
	{
		GoToHeading((RoomThreeEnterHeading +136), 8);
		//TimerWait(1000);
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);// maybe follow right wall ***************
		//printLCD("EXIT R3");
		//TimerWait(1000);
		ToRoom4();
	}
	printLCD("Exit");

}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 4 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom4(void)//go to room 4 from room 3
{
	GoToHeading((RoomThreeEnterHeading -120), 8);
	Forward(8, UNTIL_TIME, TimerMsCur() + 200);
	RWallFollow(0x37, UNTIL_LINESENSOR_LOWER, 0x40);
	GoToHeading((RoomFourEnterHeading - 8), 8);	
	Forward(8, UNTIL_TIME, TimerMsCur() + 1000);
	if (ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom4();
	}
	else
	{
	    GoToHeading((RoomFourEnterHeading+128), 8);	
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);	
		Forward(8, UNTIL_TIME, TimerMsCur() + 100);	
		RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(100);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(100);
	}
	printLCD("Exit");	
}	
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 1 Canada
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom1(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	printLCD("Exiting");		
	ExitRoom();
	GoToHeading((RoomOneEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_TIME,TimerMsCur() + 2000);//	
	printLCD("2500");		
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 2 Canada
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom2(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	printLCD("Exiting");		
	ExitRoom();
	GoToHeading((RoomTwoEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 3 Canada
*
***********************************************************************************************************/


//#########################################################################################################
 void HomeFrom3(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	printLCD("Exiting");		
	ExitRoom();
	GoToHeading((RoomThreeEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	GoToHeading((RoomFourEnterHeading +150),8);
	Forward(10,UNTIL_TIME,TimerMsCur() + 800);
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);	
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 4 Canada
*
***********************************************************************************************************/


//#########################################################################################################
 void HomeFrom4(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	printLCD("Exiting");		
	ExitRoom();
	GoToHeading((RoomFourEnterHeading +136),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 700);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################


/**********************************************************************************************************
*
* 	Leaves the room
*
***********************************************************************************************************/

void ExitRoom(void)
{

	while (GetADC(FrontLineSensor) > 0x80) // Drive until line on the floor
	{	

	//Turn sharp if too close to wall
		if ((GetADC(LeftDistSensor) > 0x38))
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(-7);
		}
		else if ((GetADC(RightDistSensor) > 0x38))
		{
			SetLeftMotorPWM(-7);
			SetRightMotorPWM(10);
		}
	//Steer out of the room	
		else if (GetADC(LeftDistSensor) > (GetADC(RightDistSensor)))	
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(5);		
		}
		else 
		{					
			SetLeftMotorPWM(5);
			SetRightMotorPWM(10);
		}
	}
//Reached the threshold
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);	
}



/**********************************************************************************************************
*
* 	Follows Right Wall
*
***********************************************************************************************************/

void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcR, RDist;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		RDist = (2000/adcR);
		if(RDist > (TargetDistance + 10))
		{
			show2hex(RDist,10);		
			SetLeftMotorPWM(7);
			SetRightMotorPWM(1);
		}			
		else if(RDist > (TargetDistance + 4))
		{
			show2hex(RDist,4);
			SetLeftMotorPWM(8);
			SetRightMotorPWM(1);
		}
		else if(RDist > (TargetDistance + 2))
		{
			show2hex(RDist,2);	
			SetLeftMotorPWM(9);
			SetRightMotorPWM(8);
		}			
		else if(RDist < (TargetDistance - 10))
		{
			show2hex(RDist,10);		
			SetLeftMotorPWM(1);
			SetRightMotorPWM(7);
		}	
		else if(RDist < (TargetDistance - 4))
		{
			show2hex(RDist,4);	
			SetLeftMotorPWM(1);
			SetRightMotorPWM(8);
		}			
		else if(RDist < (TargetDistance - 2))
		{	
			show2hex(RDist,2);
			SetLeftMotorPWM(8);
			SetRightMotorPWM(9);
		}					
		else
		{
			show2hex(RDist,00);
			SetLeftMotorPWM(9);
			SetRightMotorPWM(9);
		}
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
/*
void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcR, RDist, LDist;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcL = GetADC(LeftDistSensor);
		RDist = (2000/adcR);
		LDist = (2000/adcL);
		//sendNumber(adcL);
		//TimerWait(1);
		//sendChar(',');
		//TimerWait(1);
		//sendNumber(adcM);
		//TimerWait(1);
		//sendChar(',');
		//TimerWait(1);
		//sendNumber(adcR);
		//TimerWait(5);
		//sendString("\r\n");
		//TimerWait(50);
		//if(adcM > 0x2A)
		//{
		//	SetLeftMotorPWM(3);
		//	SetRightMotorPWM(10);
		//}	

		if(RDist > (TargetDistance + 10))
		{
			//printLCD("+10");		
			SetLeftMotorPWM(8);
			SetRightMotorPWM(3);
		}			
		else if(RDist > (TargetDistance + 6))
		{
			//printLCD("+6");
			SetLeftMotorPWM(7);
			SetRightMotorPWM(1);
		}
		else if(RDist > (TargetDistance + 2))
		{
			//printLCD("+4");		
			SetLeftMotorPWM(7);
			SetRightMotorPWM(6);
		}			
		else if(RDist < (TargetDistance - 10))
		{
			//printLCD("-10");		
			SetLeftMotorPWM(-1);
			SetRightMotorPWM(7);
		}	
		else if(RDist < (TargetDistance - 6))
		{
			//printLCD("-6");		
			SetLeftMotorPWM(1);
			SetRightMotorPWM(7);
		}			
		else if(RDist < (TargetDistance - 2))
		{	
			//printLCD("-4");	
			SetLeftMotorPWM(5);
			SetRightMotorPWM(7);
		}					
		else
		{
			//printLCD(" ");
			SetLeftMotorPWM(8);
			SetRightMotorPWM(8);
		}
		


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}*/

/*This is the propotional code

void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR, RDist, LDist, diff;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		RDist = (2000/adcR);
		LDist = (2000/adcL);
		
		if(RDist > TargetDistance )
		{
			diff = (RDist - TargetDistance)/2;	
			show2hex((8-diff), 8);
			SetLeftMotorPWM(8-diff);
			SetRightMotorPWM(8);
		}		

		else if(adcR < TargetDistance)
		{
			diff = (TargetDistance - RDist)/2; 	
			show2hex(8,(8-diff));
			SetLeftMotorPWM(8);
			SetRightMotorPWM(8-diff);
		}
		else
		{
			show2hex(8,8);
			SetLeftMotorPWM(8);
			SetRightMotorPWM(8);
		}		
	


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
*/

/**********************************************************************************************************
*
* 	Follows Left Wall
*
***********************************************************************************************************/



void LWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
unsigned char adcL, LDist;
   
	do
	{
		adcL = GetADC(LeftDistSensor);
		LDist = (2000/adcL);

		if(LDist > (TargetDistance + 10))
		{
			//printLCD("+10");		
			SetLeftMotorPWM(1);
			SetRightMotorPWM(7);
		}			
		else if(LDist > (TargetDistance + 4))
		{
			//printLCD("+4");
			SetLeftMotorPWM(1);
			SetRightMotorPWM(8);
		}
		else if(LDist > (TargetDistance + 2))
		{
			//printLCD("+2");		
			SetLeftMotorPWM(8);
			SetRightMotorPWM(9);
		}			
		else if(LDist < (TargetDistance - 10))
		{
			//printLCD("-10");		
			SetLeftMotorPWM(7);
			SetRightMotorPWM(1);
		}	
		else if(LDist < (TargetDistance - 4))
		{
			//printLCD("-4");		
			SetLeftMotorPWM(8);
			SetRightMotorPWM(1);
		}			
		else if(LDist < (TargetDistance - 2))
		{	
			//printLCD("-2");	
			SetLeftMotorPWM(9);
			SetRightMotorPWM(8);
		}					
		else
		{
			//printLCD("10");
			SetLeftMotorPWM(9);
			SetRightMotorPWM(9);
		}
		


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}	



/**********************************************************************************************************
*
* 	A FireFighting Demo For Showing Off
*
***********************************************************************************************************/

void FlameDemo(void)
{
	while (!(getkey() == 1))
	{
	unsigned char	HotPixel,
					Temperature;
		GetHotPixel(&HotPixel,&Temperature);

		if(Temperature > 6) // If there is a flame, returns true
		{
			show2hex(HotPixel,Temperature);
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			if (HotPixel == 1)
			{
				//printLCD("pixel 1");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(7);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 1");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 2 )
			{
				//printLCD("pixel 2");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(8);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 2");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 3 )
			{
			//	printLCD("pixel 3");
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(9);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 3");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");			
			}			
			else if (HotPixel == 4 )
			{
			//	printLCD("pixel 4");
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 4");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 5 )
			{
			//	printLCD("pixel 5");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 5");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 6)
			{
				//printLCD("pixel 6");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(9);
				TimerWait(10);
				sendString("Pixel 6");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 7)
			{
				//printLCD("pixel 7");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(8);
				TimerWait(10);
				sendString("Pixel 7");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}		
			else if (HotPixel == 8)
			{
				//printLCD("pixel 8");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(7);
				TimerWait(10);
				sendString("Pixel 8");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}

			/*if(PINB & 0x20)				// check for balloon pop
			{
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(1000);
				break;
			*/

			else
			{
				break;
			}
		}
		else
		{
			SetRightMotorPWM(7);
			SetLeftMotorPWM(-7);
			printLCD("no");
			sendString("No");		
			sendString("\r\n");
		}
	}
}




/**********************************************************************************************************
*
* 	Terminating Conditions - a while loop with diffrent terminating conditions.
*
***********************************************************************************************************/

int CheckTerminatingConditions(int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char LDist, MDist, RDist;

	RDist = (2000/GetADC(RightDistSensor));
	MDist = (2000/GetADC(MiddleDistSensor));
	LDist = (2000/GetADC(LeftDistSensor));
	
	switch(TerminatingCondition)
	{
	case UNTIL_KEYPRESS:
		if(getkey() == TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_GREATER:
		if(GetADC(RightDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_GREATER:
		if(GetADC(MiddleDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_GREATER:
		if(GetADC(LeftDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_GREATER:
		if(GetADC(FrontLineSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_LOWER:
		if(GetADC(RightDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_LOWER:
		if((2000/GetADC(MiddleDistSensor)) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_LOWER:
		if(GetADC(LeftDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_LOWER:
		if(GetADC(FrontLineSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_TIME:
		if(TimerMsCur() > TerminatingValue)
			return(TRUE);
		
		break;
	}

	return(FALSE);

}




/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Fire Fighting Tests

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/




/**********************************************************************************************************
*
* 	Threshold Test - Tests the Line Sensor by driving and sending out the sensor value.
*
***********************************************************************************************************/

//##########################################################################################################
void Thresholdtest(void)
{
	
	unsigned char adcLine;
	
    while (!(getkey() == 1))
    { 
	SetLeftMotorPWM(10);
    SetRightMotorPWM(10);
	adcLine = GetADC(FrontLineSensor);
	show1hex(adcLine);	
	sendNumber(adcLine);
	sendString("\r\n");
	TimerWait(100);
	}

}
//##########################################################################################################





/**********************************************************************************************************
*
*	 Motor Test - Tests the Motors By Driving them in diffrent directions.
*
***********************************************************************************************************/

//##########################################################################################################
void PWM_Test(void)
{
    while (!(getkey() == 1))
    { 	 
	
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(10);
        TimerWait(500);
		
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(0);
        TimerWait(500);

	    SetLeftMotorPWM(10);
        SetRightMotorPWM(10);
        TimerWait(500);
		
	
	}
    // Done
		SetLeftMotorPWM(0);
        SetRightMotorPWM(0);
}
//##########################################################################################################




/**********************************************************************************************************
*
*	 Heading test - Turns to diffrent points using the compass
*
***********************************************************************************************************/

//##########################################################################################################
void HeadingTest(void) 
{
	unsigned char TargetHeading = 255 + 60;
	while (1)
	{
	GoToHeading(TargetHeading,5);
	//show1hex(Heading);
	/*TimerWait(2000);
	
	TargetHeading = 0xC0;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);

	TargetHeading = 0x20;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);

	TargetHeading = 0x60;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);

	TargetHeading = 0x20;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);

	TargetHeading = 0xA8;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);

	TargetHeading = 0x28;
	GoToHeading(TargetHeading,10);
	show1hex(Heading);
	TimerWait(2000);*/
	}

}
//##########################################################################################################





/**********************************************************************************************************
*
*	 Turn Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
void TurnTest(void)
{
	unsigned char	TargetHeading;


	TargetHeading	= ReadCompass() ? Heading + 64 : 0;

	while (!(getkey() == 1))
	{
		if(ReadCompass())
		{
			show1hex(Heading);

			if(Heading < TargetHeading)
			{
				SetRightMotorPWM(8);
				SetLeftMotorPWM(-8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(Heading > TargetHeading)
			{
				SetRightMotorPWM(-8);
				SetLeftMotorPWM(8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(-10 < (Heading-TargetHeading) && 10 > (Heading-TargetHeading))
			{
				printLCD("Home");
				TargetHeading    += 64;
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(3000);
			}
		}
		else
		{
			printLCD("Error");
		}
	}
}
//##########################################################################################################




/**********************************************************************************************************
*
*	 Line Sensor Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
void LineDisplay(void)
{
	printLCD("Sending To Computer");
	unsigned char adcLine;
	while (!(getkey() == 1))
	{
		adcLine = GetADC(FrontLineSensor);
		show1hex(adcLine);
		sendNumber(adcLine);
		sendString("\r\n");
		//TimerWait(200);
	}
}
//##########################################################################################################



/**********************************************************************************************************
*
*	 Line Test - Drives until line.
*
***********************************************************************************************************/

//##########################################################################################################
void LineTest(void)
{
	unsigned char adcLine;
	while (!(getkey() == 1))
	{
		adcLine = GetADC(FrontLineSensor);
		show1hex(adcLine);
		SetLeftMotorPWM(10);
		SetRightMotorPWM(10);
		
		if(adcLine < 0xEE)
		{
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);	
		TimerWait(2000);	
		}		
	}
}
//##########################################################################################################
//#####
//#####			GoToFlame
//#####
//##########################################################################################################
void GoToFlame(void)						
//will go to the hot pixel
{
	unsigned char	HotPixel,
					Temperature,	
					adcLine;
					
	adcLine = GetADC(FrontLineSensor);
			
	while (adcLine > 0x60)
	{
		GetHotPixel(&HotPixel,&Temperature);
		adcLine = GetADC(FrontLineSensor);
	
		if(Temperature > 6)
		{
			show2hex(HotPixel,Temperature);
			//sendNumber(HotPixel);
			//sendChar(',');
			//sendNumber(Temperature);
			//sendString("\r\n");
			if (HotPixel == 1)
			{
				//LCD_puts_f(PSTR("pixel 1"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(0);
				SetLeftMotorPWM(4);
				//TimerWait(10);
			}
			else if (HotPixel == 2 )
			{
				//LCD_puts_f(PSTR("pixel 2"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(1);
				SetLeftMotorPWM(4);
				//TimerWait(10);
			}
			else if (HotPixel == 3 )
			{
			//	LCD_puts_f(PSTR("pixel 3"),1);
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(2);
				SetLeftMotorPWM(6);
				//TimerWait(10);
			}			
			else if (HotPixel == 4 )
			{
			//	LCD_puts_f(PSTR("pixel 4"),1);
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(4);
				SetLeftMotorPWM(6);
				//TimerWait(10);
			}						
			else if (HotPixel == 5 )
			{
			//	LCD_puts_f(PSTR("pixel 5"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(6);
				SetLeftMotorPWM(4);
				//TimerWait(10);
			}
			else if (HotPixel == 6)
			{
				//LCD_puts_f(PSTR("pixel 6"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(6);
				SetLeftMotorPWM(2);
				//TimerWait(10);
			}						
			else if (HotPixel == 7)
			{
				//LCD_puts_f(PSTR("pixel 7"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(4);
				SetLeftMotorPWM(1);
				//TimerWait(10);
			}		
			else if (HotPixel == 8)
			{
				//LCD_puts_f(PSTR("pixel 8"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(4);
				SetLeftMotorPWM(0);
				//TimerWait(10);
			}

			if(PINB & 0x20)				// check for balloon pop
			{
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				LCD_puts_f(PSTR("Popped"),1);
				TimerWait(1000);
				return;
			}
		}
		else
		{
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);
			LCD_puts_f(PSTR("Error"),1);
		}
	}
		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);
		LCD_puts_f(PSTR("White Circle"),1);
		TimerWait(10000);
}

	



